#include <iostream>
#include <boost/circular_buffer.hpp>
#include <opencv2/opencv.hpp>
#include <limits>
#include "util.h" // linearXSmooth5 quadraticSmooth7
using namespace std;
int second = 10;
#define HEIGHT_ 640
#define WIDTH_  960
#define FILER_SIZE 50
//!< y = 0.3*x + 7
double get_y_linearX(double x)
{
    return 0.3*x + 0.17 + (rand()%100)*0.5 ;
}

//!< y = 0.3*x*x + 7
double get_y(double x)
{
    return 40*sin(20*x) + 12 + (rand()%100)*0.87 ;
}

typedef boost::circular_buffer<cv::Point2f> M_QUEYE;
int drawData(const M_QUEYE& dataX, const M_QUEYE& linearX,cv::Mat& showDataX)
{
    double minx = std::numeric_limits<double>::max();
    double miny = std::numeric_limits<double>::max();
    double maxx = std::numeric_limits<double>::min();
    double maxy = std::numeric_limits<double>::min();
    double maxDiff = std::numeric_limits<double>::min();
    size_t maxDiffIndex = 0;
    for (size_t i = 0; i < dataX.size(); ++i) {
        if(dataX[i].x < minx) minx = dataX[i].x;
        if(dataX[i].y < miny) miny = dataX[i].y;
        if(dataX[i].x > maxx) maxx = dataX[i].x;
        if(dataX[i].y > maxy) maxy = dataX[i].y;

        if(linearX[i].x < minx) minx = linearX[i].x;
        if(linearX[i].y < miny) miny = linearX[i].y;
        if(linearX[i].x > maxx) maxx = linearX[i].x;
        if(linearX[i].y > maxy) maxy = linearX[i].y;
        if(fabs(linearX[i].y-dataX[i].y)>maxDiff) {
            maxDiff = fabs(linearX[i].y-dataX[i].y);
            maxDiffIndex = i;
        }
    }
    if(maxDiff>0.5) second = 0;
    double diffx = (maxx - minx), diffy = (maxy - miny);
    double rateX = WIDTH_/diffx, rateY = HEIGHT_/diffy;

    cv::Point2f last, cur, lastF, curF;
    for (size_t i = 0; i < dataX.size(); ++i) {
        cur.x = (dataX[i].x - minx)*rateX;               cur.y = (dataX[i].y - miny)*rateY;
        cv::circle(showDataX,cur,2,cv::Scalar(0,0,0)); cv::line(showDataX,last,cur,cv::Scalar(0,0,0));
        last = cur;

        curF.x = (linearX[i].x - minx)*rateX;                curF.y = (linearX[i].y - miny)*rateY;
        cv::circle(showDataX,curF,3,cv::Scalar(255,255,45));  cv::line(showDataX,lastF,curF,cv::Scalar(255,255,45));
        lastF = curF;
    }
    cv::putText(showDataX,"max: "+to_string(diffy),cv::Point2d(400,50),4,1,cv::Scalar( 0, 0, 0 ), 4);
    cv::putText(showDataX,"maxDiff: "+to_string(maxDiff)+" index: "+to_string(maxDiffIndex) ,cv::Point2d(200,100),4,1,cv::Scalar( 255, 0, 0 ), 4);
    cv::line(showDataX,
             cv::Point2f((dataX[maxDiffIndex].x - minx)*rateX,(dataX[maxDiffIndex].y - miny)*rateY),
             cv::Point2f((linearX[maxDiffIndex].x - minx)*rateX,(linearX[maxDiffIndex].y - miny)*rateY),
             cv::Scalar(0,0,201));
}


int main(int argc, char *argv[])
{
    M_QUEYE dataX(50);
    M_QUEYE GPSX(50);
    M_QUEYE dataY(50);
    M_QUEYE GPSY(50);

    uchar key = 0;
    ifstream ifs("ndt.log");
    ResultData ndt;

    while (key!=27) {
        ifs >> ndt;
        cout << ndt;
        if(ndt.timeStamp < atoi(argv[1])) continue;
        cv::Mat showDataX(HEIGHT_,WIDTH_,CV_8UC3,cv::Scalar(255,255,255));
        cv::Mat showDataY(HEIGHT_,WIDTH_,CV_8UC3,cv::Scalar(255,255,255));

        cv::Point2f newPointX(ndt.timeStamp*20,ndt.resultX);
        cv::Point2f newPointY(ndt.timeStamp*20,ndt.resultY);
        dataX.push_back(newPointX); dataY.push_back(newPointY);
        cv::Point2f gpsPointX(ndt.timeStamp*20,ndt.diffX);
        cv::Point2f gpsPointY(ndt.timeStamp*20,ndt.diffY);
        GPSX.push_back(gpsPointX); GPSY.push_back(gpsPointY);

        drawData(dataX,GPSX,showDataX); drawData(dataY,GPSY,showDataY);
        cv::imshow("graphX",showDataX);    cv::imshow("graphY",showDataY);
        key = cv::waitKey(second);
        if(key == 's') second = 0;
        else second = 10;
    }

    return 0;
}


int testMemory()
{
    boost::circular_buffer<cv::Mat> memory(400);
    for (int i = 0; i < 10000; ++i) {
        cv::Mat src = cv::imread("1.png");
        memory.push_back(src);
        cout << i << ": " << memory.size() << endl;
        usleep(1000);
    }
    return 0;
}
